/**
 * @file   opencv_ctrl.h
 * @brief  opencv
 * @encode UTF-8
 * @log    date        author      notes
 *         2021.09.07  Jimmy       first version
 */
#include <iostream>
#include <thread>
#include <pthread.h>
#include <mutex>
#include <memory>
#include "debug_log/log.h"
#include "utils/date_time.h"
#include "simulation.h"

Simu * Simu::mInstance = nullptr;

#define SIMULATION            Simu::Instance()

/* Opencv窗口坐标
 * (0, 0) +-------------->  x
 *        |
 *        |
 *        |
 *        |
 *        |
 *      y v
 */

/*
 *  世界坐标
 *
 *  ^ y
 *  |
 *  |
 *  |
 *  |
 *  +---------------->  x
 * (0, 0)
 */

// static void mouseCallback(int event, int x, int y, int flags, void * userdata);

/**
 * @brief  Construct a new Open Cv:: Open Cv object          
 * @author Jimmy 
 * @date   2021.09.07
 */
Simu::Simu()
{
    mInstance = this;

    mShowThreadRun   = false;
    mListenThreadRun = false;
    mIsPathSet       = false;

    mCarPos.x = 500;
    mCarPos.y = 500;
    mCarPhi   = 0;

    if (loadImage() == true)
    {
        cv::setMouseCallback("simulation", mouseCallback, (void *)(&mMatShow));

        mShowThreadRun   = true;
        mListenThreadRun = true;

        std::unique_ptr<std::thread> showT = std::make_unique<std::thread> (&Simu::showThread, this);
        showT->detach();

        std::unique_ptr<std::thread> listenT = std::make_unique<std::thread> (&Simu::listenKeyThread, this);
        listenT->detach();
    }
}

/**
 * @brief  Construct a new Simu:: Simu object
 * @param  path             
 * @param  winName          
 * @author Jimmy 
 * @date   2021.09.08
 */
Simu::Simu(std::string path, std::string winName)
{
    mInstance = this;

    mImgPath = path;
    mWinName = winName;

    mShowThreadRun   = false;
    mListenThreadRun = false;
    mIsPathSet       = false;

    mCarPos.x = 500;
    mCarPos.y = 500;
    mCarPhi   = 0;

    if (loadImage() == true)
    {
        cv::setMouseCallback(mWinName, mouseCallback, (void *)(&mMatShow));

        mShowThreadRun   = true;
        mListenThreadRun = true;

        std::unique_ptr<std::thread> showT = std::make_unique<std::thread> (&Simu::showThread, this);
        showT->detach();

        std::unique_ptr<std::thread> listenT = std::make_unique<std::thread> (&Simu::listenKeyThread, this);
        listenT->detach();
    }
}

/**
 * @brief  Destroy the Open Cv:: Open Cv object
 * @author Jimmy 
 * @date   2021.09.07
 */
Simu::~Simu()
{
    mShowThreadRun   = false;
    mListenThreadRun = false;
}


/**
 * @brief  加载图片
 * @return true 
 * @return false 
 * @author Jimmy 
 * @date   2021.09.07
 */
bool Simu::loadImage(void)
{
    cv::Mat src = cv::imread(mImgPath, 1);

    mIsStartSet = mIsEndSet = false;

    if (src.empty())
    {
        log_shell::LOG_ERROR("Simulation Read Map Failed\r\n");

        return false;
    }

    cv::Size outSize;

    mScaler = (src.cols >= src.rows) ? (1000.0 / src.cols) : (1000 / src.rows);
    log_shell::LOG_INFO("Current Scaler:%f\r\n", mScaler);
    outSize.width = src.cols * mScaler;
    outSize.height= src.rows * mScaler;
    cv::resize(src, mMatSrc, outSize, 0, 0, cv::INTER_AREA);
    mMatShow = mMatSrc.clone();

    mShowWidth  = mMatShow.cols;
    mShowHeight = mMatShow.rows;

    if (mScaler >= 1.0)
    {
        /* 图像放大 */
        cv::cvtColor(src, mMatGray, cv::COLOR_RGBA2GRAY);
    }
    else
    {
        /* 图像缩小 */
        cv::cvtColor(mMatSrc, mMatGray, cv::COLOR_RGBA2GRAY);
    }
    
    cv::imshow(mWinName, mMatShow);

    mMapData.param.width  = mMatGray.cols;
    mMapData.param.height = mMatGray.rows;
    mMapData.map.resize(mMapData.param.width, mMapData.param.height);

    mGrayWidth  = mMapData.param.width;
    mGrayHeight = mMapData.param.height;

    log_shell::LOG_INFO("Gray Map Size:(%d x %d)\r\n", mGrayHeight, mGrayWidth);

    mMapData.map.clear();
    for (uint16_t i = mMapData.param.height; i > 0; i --)
    {
        unsigned char * data = mMatGray.ptr<unsigned char> (i);

        for (uint16_t j = 0; j < mMapData.param.width; j ++)
        {
            uint8_t pix = (uint8_t)data[j];
            mMapData.map.push_back(pix);
        }
    }

    // testImage();

    return true;
}


/**
 * @brief  显示线程
 * @author Jimmy 
 * @date   2021.09.08
 */
void Simu::showThread(void)
{
    while (mShowThreadRun)
    {
        display();
        mSleep(50);
    }    
}

/**
 * @brief  监听线程
 * @author Jimmy 
 * @date   2021.09.08
 */
void Simu::listenKeyThread(void)
{
    while (mListenThreadRun)
    {
        listenKey(100);
    }
}

/**
 * @brief  显示
 * @author Jimmy 
 * @date   2021.09.08
 */
void Simu::display(void)
{
    mMatShow = mMatSrc.clone();


    drawGrid(mMatShow);
    
    if (mIsPathSet)
    {
        drawPath();
    }

    

    if (mIsStartSet)
    {
        drawStartPoint(mStartPos);
    }

    if (mIsEndSet)
    {
        drawEndPoint(mEndPos);
    }    

    // drawCarrier();

    cv::imshow(mWinName, mMatShow);
}

/**
 * @brief  监听按键
 * @param  sleep            
 * @author Jimmy 
 * @date   2021.09.08
 */
void Simu::listenKey(uint16_t sleep)
{
    char key;

    if ((int)(key = cv::waitKey(sleep)) > 0)
    {
        if (key == 's' || key == 'S')
        {
            log_shell::LOG_INFO("Press S, Start Pathing\r\n");

            if (!mIsStartSet || !mIsEndSet)
            {
                log_shell::LOG_ERROR("start point or end point not set\r\n");
            }
            else
            {
                mSimEventTigger = true;
                mSimEvent = sim_shell::SME_PATHING;

                if (mEventCallback != nullptr)
                {
                    mEventCallback(mSimEvent);
                }
            }
        }
        else if (key == 'c' || key == 'C')
        {
            log_shell::LOG_INFO("Press C, Clear All\r\n");

            mIsStartSet = false;
            mIsEndSet   = false;
            mIsPathSet  = false;
            mPath.clear();
            mShowPath.clear();
        }
    }
}

/**
 * @brief  设置起点
 * @param  startP           
 * @author Jimmy 
 * @date   2021.09.07
 */
void Simu::setStartPoint(Point startP)
{
    mStartPos   = startP;
    mIsStartSet = true;
}

/**
 * @brief  设置终点
 * @param  endP             
 * @author Jimmy 
 * @date   2021.09.07
 */
void Simu::setEndPoint(Point endP)
{
    mEndPos   = endP;
    mIsEndSet = true;
}

/**
 * @brief  mat读取测试
 * @author Jimmy 
 * @date   2021.09.08
 */
void Simu::testImage(void)
{
    cv::Mat mat = cv::Mat(1000, 1000, CV_8UC3, cv::Scalar(255,255,255));

    log_shell::LOG_INFO("width:%d, height:%d\r\n", mMapData.param.width, mMapData.param.height);

    for (int i = 0; i < mMapData.param.height; i ++)
    {
        for (int j = 0; j < mMapData.param.width; j ++)
        {
            int index = i * mMapData.param.width + j; 

            mat.at<cv::Vec3b>((mMapData.param.height - i), j)[0] = (mMapData.map[index] & 0x00FF);
            mat.at<cv::Vec3b>((mMapData.param.height - i), j)[1] = (mMapData.map[index] & 0x00FF);
            mat.at<cv::Vec3b>((mMapData.param.height - i), j)[2] = (mMapData.map[index] & 0x00FF);
            mat.at<cv::Vec3b>((mMapData.param.height - i), j)[3] = (mMapData.map[index] & 0x00FF);
        }
    }

    cv::imshow("test mat", mat);
}

/**
 * @brief  在窗口中绘制地图的栅格
 * @author Jimmy 
 * @date   2021.09.16
 * @note   仅在图像放大时有效
 */
void Simu::drawGrid(cv::Mat & mat)
{
    if (mScaler < 1.0)
    {
        return;
    }

    for (int i = mScaler; i < (int)(mShowHeight); i += (int)mScaler)
    {
        cv::line(mat, cv::Point(0, i), cv::Point(mShowWidth, i), cv::Scalar(30, 30, 30, 50), 1);
    }

    for (int i = mScaler; i < (int)(mShowWidth); i += (int)mScaler)
    {
        cv::line(mat, cv::Point(i, 0), cv::Point(i, mShowHeight), cv::Scalar(30, 30, 30, 50), 1);
    }
}

/**
 * @brief  在图上画开始点
 * @param  p                
 * @author Jimmy 
 * @date   2021.09.07
 */
void Simu::drawStartPoint(Point p)
{
    cv::Point drawP;
    drawP.x = p.x;
    drawP.y = p.y;

    int pointSize = 5;

    if (mScaler > 1.0)
    {
        drawP.x = (int)(p.x / mScaler);
        drawP.y = (int)(p.y / mScaler);

        drawP.x = (int)(drawP.x * mScaler);
        drawP.y = (int)(drawP.y * mScaler);

        drawP.x = (int)(drawP.x + int(mScaler / 2));
        drawP.y = (int)(drawP.y + int(mScaler / 2));

        pointSize = (int)(mScaler / 1.5 + 0.5);
    }
    /* cv::Scalar(B, G, R, 透明度) */
    cv::circle(mMatShow, drawP, pointSize, cv::Scalar(0, 255, 0, 100), -1);
}

/**
 * @brief  在图上画结束点
 * @param  p                
 * @author Jimmy 
 * @date   2021.09.07
 */
void Simu::drawEndPoint(Point p)
{
    int rectSize = 10;

    cv::Point drawP;
    drawP.x = p.x;
    drawP.y = p.y;

    if (mScaler > 1.0)
    {
        drawP.x = (int)(p.x / mScaler);
        drawP.y = (int)(p.y / mScaler);

        drawP.x = (int)(drawP.x * mScaler);
        drawP.y = (int)(drawP.y * mScaler);

        drawP.x = (int)(drawP.x);
        drawP.y = (int)(drawP.y);

        rectSize = (int)(mScaler + (int)(mScaler/5) + 0.5);
    }

    /* cv::Scalar(B, G, R, 透明度) */
    // cv::circle(mMatShow, drawP, 5, cv::Scalar(0, 0, 255, 100), -1);
    cv::rectangle(mMatShow, cv::Rect(drawP.x - 5, drawP.y - 5, rectSize, rectSize), cv::Scalar(0, 0, 255, 100), -1);
}

/**
 * @brief  绘制路径
 * @author Jimmy 
 * @date   2021.09.09
 */
void Simu::drawPath()
{
    if (mPath.empty() == false)
    {
        mShowPath.push_back(mPath.back());
        mPath.pop_back();
    }

    uint16_t pathSize = mShowPath.size();
    int      pathLineWidth = 2;

    for (uint16_t i = 0; i < pathSize - 1; i ++)
    {
        cv::Point startPoint;
        cv::Point endPoint;

        startPoint.x = mShowPath[i].x;
        /* 将世界坐标转换成opencv窗口坐标 */
        startPoint.y = mGrayHeight - mShowPath[i].y;

        endPoint.x = mShowPath[i + 1].x;
        /* 将世界坐标转换成opencv窗口坐标 */
        endPoint.y = mGrayHeight - mShowPath[i + 1].y;

        if (mScaler > 1.0)
        {
            startPoint.x *= (int)mScaler;
            startPoint.y *= (int)mScaler;

            endPoint.x *= (int)mScaler;
            endPoint.y *= (int)mScaler;

            startPoint.x += (int)(mScaler/2);
            startPoint.y += (int)(mScaler/2);

            endPoint.x += (int)(mScaler/2);
            endPoint.y += (int)(mScaler/2);

            pathLineWidth = (int)(mScaler/2);
        }

        cv::line(mMatShow, startPoint, endPoint, cv::Scalar(190, 190, 190, 100), (int)pathLineWidth);
        
        // cv::circle(mMatShow, startPoint, 5, cv::Scalar(100, 100, 100, 100), -1);
    }
}

/**
 * @brief  获取地图
 * @param  mapData          
 * @author Jimmy 
 * @date   2021.09.09
 */
void Simu::getMap(MapData & mapData)
{
    mapData = mMapData;
}

/**
 * @brief  发布路径
 * @param  path             
 * @author Jimmy 
 * @date   2021.09.09
 */
void Simu::pubPath(std::vector<Point> path)
{
    mShowPath.clear();
    while (!path.empty())    
    {
        mPath.push_back(path.back());
        path.pop_back();
    }
    mIsPathSet = true;
}

/**
 * @brief  获取开始点
 * @return Point 
 * @author Jimmy 
 * @date   2021.09.09
 * @note   获取到的是世界坐标下的起点
 */
Point Simu::getStartPoint(void)
{
    /* 将opecv窗口坐标转换成世界坐标 */
    Point cvtPoint;
    cvtPoint.x = mStartPos.x;
    cvtPoint.y = mShowHeight - mStartPos.y;

    if (mScaler > 1.0)
    {
        cvtPoint.x = (int)(cvtPoint.x / mScaler);
        cvtPoint.y = (int)(cvtPoint.y / mScaler) + 1;
    }

    return cvtPoint;
}


/**
 * @brief  获取结束点
 * @return Point 
 * @author Jimmy 
 * @date   2021.09.09
 * @note   获取到的是世界坐标下的终点
 */
Point Simu::getEndPoint(void)
{
    /* 将opecv窗口坐标转换成世界坐标 */
    Point cvtPoint;
    cvtPoint.x = mEndPos.x;
    cvtPoint.y = mShowHeight - mEndPos.y;

    if (mScaler > 1.0)
    {
        cvtPoint.x = (int)(cvtPoint.x / mScaler);
        cvtPoint.y = (int)(cvtPoint.y / mScaler) + 1;
    }

    return cvtPoint;
}

/**
 * @brief  事件监听
 * @return true 
 * @return false 
 * @author Jimmy 
 * @date   2021.09.09
 */
bool Simu::eventListen(void)
{
    if (mSimEventTigger)
    {
        mSimEventTigger = false;
        return true;
    }

    return false;
}


/**
 * @brief  鼠标事件
 * @param  event            
 * @param  x                
 * @param  y                
 * @param  flags            
 * @param  userdata         
 * @author Jimmy 
 * @date   2021.09.07
 */
void Simu::mouseCallback(int event, int x, int y, int flags, void * userdata)
{
    if (event == cv::EVENT_LBUTTONDOWN)
    {
        log_shell::LOG_INFO("Mouse LButton:%d, %d\r\n", x, y);
        
        SIMULATION->setStartPoint(Point(x, y));

        if (SIMULATION->getMatScaler() > 1.0)
        {
            log_shell::LOG_INFO("Cvt To Point:(%d, %d)\r\n", (int)(x/SIMULATION->getMatScaler()), (int)(y/SIMULATION->getMatScaler()));
        }

        // int cvtX, cvtY;
        // cvtX = (int)(x/SIMULATION->getMatScaler());
        // cvtY = (int)(y/SIMULATION->getMatScaler());

        // int index = (20 - cvtY) * 20 + cvtX;
        // uint8_t data = SIMULATION->getMapPixel(index);
        // log_shell::LOG_INFO("Pixel data:%d\r\n", data);

    }
    else if (event == cv::EVENT_RBUTTONDOWN)
    {
        log_shell::LOG_INFO("Mouse RButton:%d, %d\r\n", x, y);
        SIMULATION->setEndPoint(Point(x, y));

        if (SIMULATION->getMatScaler() > 1.0)
        {
            log_shell::LOG_INFO("Cvt To Point:(%d, %d)\r\n", (int)(x/SIMULATION->getMatScaler()), (int)(y/SIMULATION->getMatScaler()));
        }
    }
}


/**
 * @brief  画机器人
 * @author Jimmy 
 * @date   2021.09.09
 */
void Simu::drawCarrier(void)
{
    cv::Point carPos;
    carPos.x = mCarPos.x;
    carPos.y = mCarPos.y;

    cv::Point lidarPos;
    float linCos = -0.6366 * fabs(mCarPhi) + 1;
    float linSin = 0.0;

    if (mCarPhi >= -N_PI_2 && mCarPhi <= N_PI_2)
    {
        linSin = 0.6366 * mCarPhi;
    }
    else if (mCarPhi > N_PI_2)
    {
        linSin = -0.6366 * mCarPhi + 2;
    }
    else
    {
        linSin = -0.6366 * mCarPhi - 2;
    }

    lidarPos.x = carPos.x - 4 * linCos;
    lidarPos.y = carPos.y + 4 * linSin;

    cv::circle(mMatShow, carPos,  11, cv::Scalar(120, 120, 120, 100), -1);
    cv::circle(mMatShow, lidarPos, 4, cv::Scalar(30, 30, 30, 100),    -1);
    cv::circle(mMatShow, lidarPos, 5, cv::Scalar(150, 150, 150, 100),  1);
}


/* End of file */